package edu.asu.sapa.heuristic;

import java.util.Arrays;

import edu.asu.sapa.lifted.Symbol;
import edu.asu.sapa.utils.Utility;

public class CostFunction<K> extends Symbol<K> {
	private static final int DEFAULT = 8;

	public int size;
	public float[] times= new float[DEFAULT];
	public float[] costs= new float[DEFAULT];
	public int[] supportIDs= new int[DEFAULT];

	// should keep track of supporters too, and realize when they drop to 0
	// (if we want to do eager analysis upon no longer applicable actions)
	// the other possibility is to just clear out all propositions and do
	// a re-propagation from the (new) initial action
	
	public int[] conditions = new int[8];
	public int conditionsSize = 0;

	public int[] adds = new int[8];
	public int addsSize = 0;
	
	public int[] deletes = new int[8];
	public int deletesSize = 0;
	
	//public boolean reachable;
	public float selectedTime;
	
	public CostFunction(K name) {
		super(name);
		selectedTime=Float.NaN;
	}

	public final boolean achievable(float time) {
		if (size < 1 || times[0] > time) {
			return false;
		}

		return true;
	}

	public final void select(float time) {
		if (time < selectedTime || selectedTime != selectedTime)
			selectedTime=time;
	}
	
	public final boolean selected(float time) {
		return (selectedTime <= time);
	}
	
	public final boolean selected() {
		return selectedTime == selectedTime;
	}
	
	public final int getIndex(float time) {
		for (int i=size-1;i>=0;--i)
			if (times[i] <= time)
				return i;
		return -1;
	}
	/* Manage the costs & supporting action */
	// does insertion sort, minimizing representation by enforcing strict decreasing
	public final boolean addRelaxedCost(float time, float c, int spID) {
		if (size >= costs.length) {
			int capacity = size + (size >> 1) + 1;
			costs = Arrays.copyOf(costs, capacity);
			supportIDs = Arrays.copyOf(supportIDs, capacity);
			times = Arrays.copyOf(times, capacity);
		}

		int i;
		for (i = size - 1; i >= 0; --i) {
			if (times[i] < time) {
				if (c >= costs[i]) {
					return false;
				}
				++i;
				if (i == size) {
					times[size] = time;
					costs[size] = c;
					supportIDs[size] = spID;
					++size;
					return true;
				}
				if (c == costs[i]) {
					if (time == times[i]) {
						return false;
					}
					times[i] = time;
					supportIDs[i] = spID;
				} else if (c > costs[i]) {
					if (time == times[i]) {
						return false;
					}
					Utility.insert(costs, i, c, size);
					Utility.insert(times, i, time, size);
					Utility.insert(supportIDs, i, spID, size);
					++size;
				} else if (c < costs[i]) {
					times[i] = time;
					costs[i] = c;
					supportIDs[i] = spID;
					++i;
					int j;
					for (j = i; j < size; ++j) {
						if (c >= costs[j]) {
							if (j > i) {
								Utility.remove(costs, i, j - i, size);
								Utility.remove(times, i, j - i, size);
								Utility.remove(supportIDs, i, j - i, size);
								size -= j - i;
							}
							return true;
						}
					}
					size = i;
					return true;
				}
				return true;
			}
		}
		++i;
		if (i == size) {
			times[size] = time;
			costs[size] = c;
			supportIDs[size] = spID;
			++size;
			return true;
		}
		if (c == costs[i]) {
			if (time == times[i]) {
				return false;
			}
			times[i] = time;
			supportIDs[i] = spID;
		} else if (c > costs[i]) {
			if (time == times[i]) {
				return false;
			}
			Utility.insert(costs, i, c, size);
			Utility.insert(times, i, time, size);
			Utility.insert(supportIDs, i, spID, size);
			++size;
		} else if (c < costs[i]) {
			times[i] = time;
			costs[i] = c;
			supportIDs[i] = spID;
			++i;
			int j;
			for (j = i; j < size; ++j) {
				if (c >= costs[j]) {
					if (j > i) {
						Utility.remove(costs, i, j - i,size);
						Utility.remove(times, i, j - i,size);
						Utility.remove(supportIDs, i, j - i,size);
						size -= j - i;
					}
					return true;
				}
			}
			size=i;
			return true;
		}
		return true;
	}

	
	public final boolean addRelaxedCostDelta(float time, float c, int spID) {
		if (size >= costs.length) {
			int capacity = size + (size >> 1) + 1;
			costs = Arrays.copyOf(costs, capacity);
			supportIDs = Arrays.copyOf(supportIDs, capacity);
			times = Arrays.copyOf(times, capacity);
		}

		int i;
		for (i = size - 1; i >= 0; --i) {
			if (times[i] <= time)
				break;
		}
		++i;
		if (i == size) {
			times[size] = time;
			costs[size] = c;
			supportIDs[size] = spID;
			++size;
			return true;
		}
		Utility.insert(costs, i, c, size);
		Utility.insert(times, i, time, size);
		Utility.insert(supportIDs, i, spID, size);
		++size;
		return true;
	}
	
	
	
	
	
/*	
	public final boolean addCost(float time, float cost, int id) {
		int capacity = size;
		if (capacity >= costs.length) {
			int len = capacity + (capacity >> 1) + 1;
			float[] s, d;
			int[] si,di;
		
			s = costs;
			d = new float[len];
			System.arraycopy(s,0,d,0,capacity);
			costs = d;
			
			s = times;
			d = new float[len];
			System.arraycopy(s,0,d,0,capacity);
			times = d;
			
			si = supportIDs;
			di = new int[len];
			System.arraycopy(s,0,d,0,capacity);
			supportIDs = di;
		}
		costs[capacity]=cost;
		times[capacity]=time;
		supportIDs[capacity]=id;
		++size;
		return true;
	}
	*/
	
	public void deactivate() {
		//last = 0;
		size = 0;
		selectedTime = Float.NaN;
		//reachable = false;
	}

	public final float getCurrentRelaxedCost() {
		return size > 0 ? costs[size - 1] : Float.POSITIVE_INFINITY;
	}

	public final float getRelaxedCost(float time) {
		for (int i = size - 1; i >= 0; --i) {
			if (times[i] <= time) {
				return costs[i];
			}
		}

		return Float.POSITIVE_INFINITY;
	}
	
	public float getRelaxedCostDelta(float time) {
		int i;
		for (i = size - 1; i >= 1; --i) {
			if (times[i] <= time) {
				return costs[i] - costs[i-1];
			}
		}
		if (i==0)
			return costs[0];
		return Float.POSITIVE_INFINITY;
	}

	
	public final boolean improvedBy(float time, float cost) {
		int i = 0;
		int j = size-1;
		while (i < j) {
			if (cost < costs[j])
				return true;
			if (times[j] <= time)
				return false;
			if (times[i] > time)
				return true;
			if (cost >= costs[i])
				return false;
			++i;
			--j;
		}
		// if odd, i.e., size 1, must be earlier or better.  True also for >=3 if the loop finishes
		if (i==j) {
			if (time < times[i] || cost < costs[i])
				return true;
			return false;
		}
		// if even and not 0, then the loop fired, and on the last iteration:
		// t[i] <= time < t[i+1] && c[i] > cost >= c[i+1]
		// and if 0, then improvement is also true
		return true;
	}

	/**
	 * 
	 * \argmin_t cost(t) == cost(tmax)
	 * <br>
	 * \forall t getCost(getMinTime(t)) == getCost(t)
	 * 
	 * @param tmax - a time
	 * @return The minimum time at which the relaxed cost is equivalent to the cost at time {@code tmax}.
	 */
	public final float getMinTime(float tmax) {
		for (int i = size - 1; i >= 0; --i) {
			if (times[i] <= tmax)
				return times[i];
		}

		return Float.NEGATIVE_INFINITY;
	}

	public final int getSupport() {
		if (size <= 0) {
			return -1;
		}
		return supportIDs[size - 1];
	}

	public final int getSupport(float time) {
		for (int i = size - 1; i >= 0; --i) {
			if (times[i] <= time) {
				return supportIDs[i];
			}
		}

		return Integer.MIN_VALUE;
	}

	public final boolean isIn() {
		return size > 0;
	}
	
	
	public int addCondition(int id) {
		if (conditionsSize >= conditions.length) {
			conditions = Arrays.copyOf(conditions, conditionsSize + 8 + (conditionsSize >> 2));
		}
		conditions[conditionsSize++] = id;
		return conditionsSize;
	}
	
	public int addAdd(int id) {
		if (addsSize >= adds.length) {
			adds = Arrays.copyOf(adds, addsSize + 8 + (addsSize >> 2));
		}
		adds[addsSize++] = id;
		return addsSize;
	}

	public int addDelete(int id) {
		if (deletesSize >= deletes.length) {
			deletes = Arrays.copyOf(deletes, deletesSize + 8 + (deletesSize >> 2));
		}
		deletes[deletesSize++] = id;
		return deletesSize;
	}

	public void trim() {
		if (conditionsSize != conditions.length)
			conditions = Arrays.copyOf(conditions, conditionsSize);
		if (addsSize != adds.length)
			adds = Arrays.copyOf(adds, addsSize);
		if (deletesSize != deletes.length)
			deletes = Arrays.copyOf(deletes, deletesSize);
	}

	public int removeCondition(int id) {
		// could be present multiple times (at-start,over-all,at-end condition)
		for (int i = 0; i < conditionsSize; ++i) {
			if (conditions[i] == id) {
				Utility.remove(conditions, i);
				--conditionsSize;
				--i;
			}
		}
		return conditionsSize;
	}
	
	public int removeAdd(int id) {
		// could be present multiple times (at-start,over-all,at-end condition)
		for (int i = 0; i < addsSize; ++i) {
			if (adds[i] == id) {
				Utility.remove(adds, i);
				--addsSize;
				--i;
			}
		}
		return addsSize;
	}
	
	public int removeDelete(int id) {
		// could be present multiple times (at-start,over-all,at-end condition)
		for (int i = 0; i < deletesSize; ++i) {
			if (deletes[i] == id) {
				Utility.remove(deletes, i);
				--deletesSize;
				--i;
			}
		}
		return deletesSize;
	}


}
